#ifndef SWARM_COMPUTER_HPP
#define SWARM_COMPUTER_HPP

#include <Eigen/Dense>
#include <vector>
#include <unordered_map>
#include <mutex>
#include <thread>
#include <chrono>

using Eigen::Vector3d;
using std::vector;

// 定义障碍物结构体
struct Obstacle {
    Vector3d position;  // 障碍物的位置
    double obstacle_radius;  // 障碍物的半径

    // 构造函数初始化
    Obstacle(const Vector3d& pos, double radius) 
        : position(pos), obstacle_radius(radius) {}

    Obstacle():position(Vector3d(0,0,0)), obstacle_radius(0) {}
};

class swarm_controller {
public:
    // 参数配置
    double K_attract;           // 吸引力常数
    double K_repulse;           // 排斥力常数
    double repulse_radius;      // 排斥力的作用范围
    double safe_distance;       // 安全距离
    double max_speed;           // 最大速度
    double K_attract2;          // 第二阶段引导力常数
    double repulse_radius2;     // 第二阶段排斥力半径
    double repulse_exponent2;   // 第二阶段排斥力的指数
    double guidance_force_reduction;  // 引导力缩小系数
    double distance_threshold;  // 目标距离阈值
    double repulse_exponent;

    Vector3d goal;              // 目标位置
    Vector3d current_position;  // 当前飞机的位置
    std::unordered_map<std::string, Vector3d> other_positions; // 其他飞机的位置
    // Vector3d velocity;
    // std::vector<Vector3d> other_velocities; 
    std::unordered_map<int, Obstacle> obstacles;  // 障碍物(索引，障碍物)
    std::mutex obstacles_mutex_; // 互斥锁，用于保护 obstacles_

    swarm_controller();

    void set_goal(const Vector3d& goal);
    void set_obstacles(const std::unordered_map<int, Obstacle>& obstacles);
    void add_obstacle(const Obstacle obstacle, int index);
    void set_positions(const Vector3d& current_position, const std::unordered_map<std::string, Vector3d>& other_positions);
    void set_velocities(const Eigen::Vector3d& velocity, const std::vector<Eigen::Vector3d>& other_velocities);
    void set_parameters(double K_attract, double K_repulse, double repulse_radius, 
                        double safe_distance, double repulse_exponent,
                         double max_speed, double K_attract2, 
                         double repulse_radius2, double repulse_exponent2);

    Vector3d compute_force();
    bool check_near_goal_or_detection_area(const Vector3d& position, const Vector3d& target, double detection_radius);

    bool clearObstacle(int ObIndex);
    bool ObstacleExist(int ObIndex);
    Obstacle get_obstacle(int index);
};

#endif


// ros2 topic echo /uav_swarms/drone_0_communication_server_topic yolo_detect_message/CallSeverStatus "status: true"